//=============================================================================

#include <math.h>
#include "../../../../inc/aAux.h"

//=============================================================================

aAux::aAux()
{
  pi = 3.1415926535f;
  pid = 0.017453292f;
  pidi = 57.2957795f;
  dt = 0.0f;
  version = 1.0f;
}

//=============================================================================

aVector aAux::aaCrossMulty(const aVector& vec, const aVector& vec2)
{
  aVector res;

  res.x = (vec.y*vec2.z) - (vec.z*vec2.y);
  res.y = (vec.z*vec2.x) - (vec.x*vec2.z);
  res.z = (vec.x*vec2.y) - (vec.y*vec2.x);

  return res;
}

//=============================================================================

aVector aAux::aaNormalize(const aVector& v)
{
  aVector vec(v);
  float m = aaGetMagnitude(vec);

  if (aaCompareFloat(m, 0.0f))
    return aVector(0.0f, 0.0f, 0.0f);

  vec.x /= m;
  vec.y /= m;
  vec.z /= m;

  return vec;
}

//=============================================================================

float aAux::aaGetAngle(const aVector& v, const aVector& v2)
{
  aVector vec(aaNormalize(v));
  aVector vec2(aaNormalize(v2));

  float dot = aaDotMulty(vec, vec2);

  if (dot > 1.0)
    dot = 1.0;
  if (dot < (-1.0))
    dot = -1.0;

  return (acos(dot)/**180.0/pi*/);
}

//=============================================================================

aVector aAux::aaRotateAroundVector(const aVector& v, const aVector& v2, float angle)
{
  aVector dx, dy, dz;
  aVector vec(v);
  aVector vec2(v2);
  float dot = aaDotMulty(vec, vec2);
  float angle2 = ator(angle);

  dz.x = vec2.x * dot;
  dz.y = vec2.y * dot;
  dz.z = vec2.z * dot;

  dx.x = vec.x - dz.x;
  dx.y = vec.y - dz.y;
  dx.z = vec.z - dz.z;

  dy = aaCrossMulty(dx, vec2);

  vec.x = dx.x*cos(angle2) + dy.x*sin(angle2) + dz.x;
  vec.y = dx.y*cos(angle2) + dy.y*sin(angle2) + dz.y;
  vec.z = dx.z*cos(angle2) + dy.z*sin(angle2) + dz.z;

  return vec;
}

//=============================================================================

bool aAux::aaCompareFloat(float f1, float f2)
{
  if (aaFloatAbs((f1) - (f2)) < 0.0001f)
    return 1;

  return 0;
}

//=============================================================================

bool aAux::aaCompareFloatHigh(float f1, float f2)
{
  if (aaFloatAbs(f1 - f2) < 0.00001f)
    return 1;

  return 0;
}

//=============================================================================

float aAux::aaGetLength(const aVector& vec, const aVector& vec2)
{
  float vx = vec2.x - vec.x;
  float vy = vec2.y - vec.y;
  float vz = vec2.z - vec.z;

  return sqrt(vx*vx + vy*vy + vz*vz);
}

//=============================================================================


bool aAux::aaCheckVectorIn(const aVector& p, const aVector& vec1, const aVector& vec2, const aVector& vec3)
{
  aVector tmpVector2, tmpVector3, tmpVector4;

  tmpVector2 = vec1 - p;
  tmpVector3 = vec2 - p;
  tmpVector4 = vec3 - p;

  float angle = aaGetAngle(tmpVector2, tmpVector3)*aaGetPidi();

  angle += aaGetAngle(tmpVector3, tmpVector4)*aaGetPidi();
  angle += aaGetAngle(tmpVector4, tmpVector2)*aaGetPidi();

  if ((angle >= 359.9f) && (angle <= 360.1f))
    return 1;
  //else
  //  if (angle > 360.0f)
  //    return 1;

  return 0;
}

//=============================================================================

aVector aAux::aaGetIntersection(const aVector& l1, const aVector& l2, const aVector& s1, const aVector& s2, const aVector& s3)
{
  aVector tmpVector = aaGetNormal(s1, s2, s3);

  float r1 = aaDotMulty(tmpVector, (l1 - s1));
  float r2 = aaDotMulty(tmpVector, (l2 - s1));

  tmpVector = l1 + (l2 - l1)*(-r1/(r2 - r1));

  if ((r1*r2) < 0.0) // have some interscetion
    return tmpVector;

  return aVector(0.0f, 0.0f, 0.0f);
}

//=============================================================================
//
// "...Will return the correct vector only if point of
// intersection is really on both lines lies..." - he said.
//

aVector aAux::aaGetLinesIntersection(const aVector& p11, const aVector& p12, const aVector& p21, const aVector& p22, const aVector& n)
{
  aVector tmpVector = aaGetIntersection(p21, p22, p11, p12, n);

  if (!aaCompareFloat(aaGetMagnitude(tmpVector), 0.0f))
  {
    float f1 = aaGetLength(tmpVector, p11);
    float f2 = aaGetLength(tmpVector, p12);
    float f3 = aaGetLength(p12, p11);

    if (aaCompareFloat(f1 + f2, f3))
    {
      f1 = aaGetLength(tmpVector, p21);
      f2 = aaGetLength(tmpVector, p22);
      f3 = aaGetLength(p22, p21);
          
      if (aaCompareFloat(f1 + f2, f3))
        return tmpVector;
    }
  }

  return aVector(0.0, 0.0, 0.0);
}

//=============================================================================

aVector aAux::aaGetClosestPoint(const aVector& p1, const aVector& p2, const aVector& wp)
{
  aVector tmpVector = wp - p1;
  aVector tmpVector2 = aaNormalize(p2 - p1);
  float tmpMagnitude = aaGetLength(p1, p2);
  float tmpDot = aaDotMulty(tmpVector2, tmpVector);

  if (tmpDot <= 0)
    return p1;

  if (tmpDot >= tmpMagnitude)
    return p2;

  return (p1 + tmpVector2*tmpDot);
}

//=============================================================================

aAux::~aAux()
{
}

//=============================================================================
